//Test1 per la sparkfun 6 DOF
//primo test
//lettura supercazzona, si può migliorare sensibilmente prendendo parallelamente 
//il valore dei registri di input 
unsigned long timer=0; //timer
unsigned long  dt;     //delta t - tempo esecuzione del main loop (robe sull'acqusition rate)

int value[6] = {0,0,0,0,0,0};  // dati {G y, G x, G z, A x, A y, A x} con G il gyroscopio e A l'accelerometro

void setup()
{
  analogReference(EXTERNAL);     //Setto il valore di riferimento analogico esterno (arduino-DOF)
				 //In pratica il valore di riferimento sarà il voltaggio applicato sul pin AREF. 
				 //Consigliano una resistenza da 5k sul pin (da notare il conseguente effetto di voltage divider con la resistenza interna da 32k)
  Serial.begin(115200);          
  DDRC = B00000000;              //setta tutt gli analog pin a input  TODO: tutte le altre init
  timer=millis();
  Serial.println("t[ms] \t gx \t gy \t gz \t ax \t ay \t az ");
}

void loop()
{
  dt = millis() - timer;
  timer=millis();       
  Serial.print(dt);      
  Serial.print ("\t"); 
  read_DOF();            //leggo la roba
  print_val();
  delay(20);
}

void read_DOF() //read sensor data
{
  for (int i=0; i<6; i++)
 { 
  value[i] = (analogRead(i)) ; //qui possiamo ciacciare i valore letti (trucchetti con offset calcolati o filtri vari)
                    	       //questa lettura va necessariamente ottimizzata per fare i fighi 
			       //l'alanoloRead si prende ~100 microsecondi. quindi 10000 letture al secondo max.
 //la cosa più seria da fare sarebbe utilzzare un convertitore veloce esterno multicanale (uno per pin della DOF) e beccarsi con intterupt le letture
 //esistono  A/D su I2C abbastanza economici
 }
}


void print_val() //print data
{
  Serial.print(value[1]);	
  Serial.print ("\t"); 
  Serial.print(value[0]);	
  Serial.print ("\t"); 
  Serial.print(value[2]);	
  Serial.print ("\t"); 
  Serial.print(value[5]);	
  Serial.print ("\t"); 
  Serial.print(value[4]);	
  Serial.print ("\t"); 
  Serial.print(value[3]);	
  Serial.print ("\t"); 
  Serial.println("");
}
